package com.googlecode.grs.mechanism;

import com.googlecode.grs.core.GRTObject;
import com.googlecode.grs.robot.SimpleEventMoveRobot;

/**
 * Makes the robot constantly move forward and turn when told to
 * 
 * @author Dan
 * 
 */
public class CenteringWallDriveMechanism extends GRTObject {

	private static final int TURN_STEP = 5;
	private static final int DRIVE_STEP = 1;

	private SimpleEventMoveRobot robot; // robot to move

	private CenteringWall furthest = CenteringWall.LEFT;// no reason

	private boolean turning = false;
	private boolean running = false;

	public CenteringWallDriveMechanism(SimpleEventMoveRobot robot) {
		this.robot = robot;
	}

	public void run() {
		running = true;
		while (running) {
			log("DRIVE!");

			if (furthest == CenteringWall.LEFT) {
				robot.turnLeft();
				robot.waitAngle(TURN_STEP);
			} else {
				robot.turnRight();
				robot.waitAngle(TURN_STEP);
			}

			robot.goForward();
			robot.waitDistance(DRIVE_STEP);

			try {
				sleep(10);
			} catch (InterruptedException e) {
				e.printStackTrace();
			}
		}
	}

	/**
	 * Turns the robot towards the given wall
	 * 
	 * @param wall
	 *            The wall that the robot is to turn towards
	 */
	public void turnTo(CenteringWall wall) {
		furthest = wall;

	}

}
